#include <iostream>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <Utils.h>
using namespace std;
using namespace lyxutils::abbreviations;

typedef pcl::PointXYZ PT;
typedef pcl::PointCloud<PT> CloudT;

void printUsage(){
    cout<<"usage: filter_trees folder [out_folder]"<<endl;
    cout<<"       read point cloud files from folder and filter out tree point cloud"<<endl;
    cout<<"       the remaining point cloud(mostly regularly shaped buildings) are output"<<endl;
    cout<<"       out_folder. if this folder is not given,the default `folder_out` will be used!"<<endl;
    cout<<"the principle of this method is that normal vector direction of building change smoothly"<<endl;
    cout<<"while normal vector direction of tree change radically,thus if most points' normal vector"<<endl;
    cout<<"are different from their neighbors' normal vector,then this point cloud is tree object"<<endl;
    cout<<"arguments:"<<endl;
    cout<<"  -h        print this help message"<<endl;
    cout<<"  -a=angle  set allowed angle between normal vector of neighboring points(default is 20 degree)"<<endl;
    cout<<"  -r=ratio  set the ratio threshold to filter trees(default is 0.4)"<<endl;
    cout<<"  -n=num    set number of neighbors used to compare(default is 5)"<<endl;
}

int main(int argc,char **argv) {
    xio::CLParser parser;
    parser.parse(argc,argv);
    vector<string> paras=parser.getParameters();
    if(paras.size()!=2 || parser.hasOption("h")){
        printUsage();
        return 1;
    }
    //deal with options
    float angle=parser.hasOption("a")?stof(parser.getOptionArg("a")):20.0;
    float ratio=parser.hasOption("r")?stof(parser.getOptionArg("r")):0.4;
    int num=parser.hasOption("n")?stoi(parser.getOptionArg("n")):5;
    if(!xos::directory(xos::abspath(paras[0]))){
        cout<<"directory:"<<paras[0]<<" not exist!"<<endl;
        return 1;
    }
    if(!xos::directory(xos::abspath(paras[1]))){
        xos::mkdir(xos::abspath(paras[1]));
    }
    //processing...
    ofstream ofs(paras[1]+"/log.dat");
    ofs<<"command options:"<<endl;
    ofs<<"angle="<<angle<<",ratio="<<ratio<<",num="<<num<<endl;
    ofs<<"cloud_name,total_pts,inliers,ratio,is_tree"<<endl;
    ofs<<"------------------------------------------"<<endl;
    vector<string> files=xos::lsfile(xos::abspath(paras[0]));
    int count=1;
    for(string f:files){
        if(xos::suffix(f)!="txt")continue;
        CloudT::Ptr cloud(new CloudT);
        vector<vector<float> > fields;
        xio::read_pts_field(f,cloud->points,fields);
        if(cloud->points.size()==0){
            continue;
        }
        pcl::KdTreeFLANN<PT>::Ptr tree(new pcl::KdTreeFLANN<PT>);
        tree->setInputCloud(cloud);
        int inliers=0;
        int nx=fields[0].size()-3;
        int ny=nx+1;
        int nz=ny+1;
        for(int i=0;i<cloud->points.size();++i){
            vector<int> indices;
            vector<float> sqrDis;
            tree->nearestKSearch(cloud->points[i],num+1,indices,sqrDis);
            Vec3f pNorm={fields[i][nx],fields[i][ny],fields[i][nz]};
            bool hold=true;
            for(int j=1;j<num+1;++j){
                vector<float> &row=fields[indices[j]];
                Vec3f normNghbor={row[nx],row[ny],row[nz]};
                if(acos(abs(pNorm.dot(normNghbor)/(pNorm.mold()*normNghbor.mold())))>angle*M_PI/180){
                    hold=false;
                    break;
                }
            }
            if(hold)inliers++;
        }
        float part=(float)inliers/cloud->points.size();
        if(part>ratio){//is building not tree
            ofstream outFile(xos::abspath(paras[1])+"/"+xos::fullname(f));
            for(int i=0;i<cloud->points.size();++i){
                PT &p=cloud->points[i];
                outFile<<p.x<<","<<p.y<<","<<p.z;
                for(int j=0;j<fields[i].size();++j){
                    outFile<<","<<fields[i][j];
                }
                outFile<<endl;
            }
            outFile.flush();
            outFile.close();
        }
        ofs<<f<<","<<cloud->points.size()<<","<<inliers<<","<<part<<","<<(part<ratio)<<endl;
        xio::consoleProgressBar(count++,files.size());
    }
    if(count<=files.size())xio::consoleProgressBar(100,100);
    ofs.flush();
    ofs.close();
    return 0;
}